package gridmapper.client;

import gridmapper.RobotData;
import gridmapper.RobotDataParser;
import gridmapper.RobotHeading;
import gridmapper.ServerData;
import gridmapper.ServerDataParser;

import java.io.IOException;
import java.util.Random;

import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.CompassSensor;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.robotics.navigation.TachoPilot;
import lejos.util.Delay;

public class GridMapperClient {

	//Fields
	static Motor leftMotor;
	static Motor rightMotor;
	static Motor topMotor = Motor.C;
	static CompassSensor compassSensor;
	static UltrasonicSensor ultraSonicSesnor;
	static LightSensor leftLightSensor;
	static LightSensor rightLightSensor;
	static TachoPilot myPilot;
	static Boolean iAmAlive;
	static int currentHeading = RobotHeading.NORTH;;
	static ServerDataParser myServerDataParser;
	static RobotDataParser myRobotDataParser;
	static int halfCellLength = 16; //Approximation in inches of half cell length
	static int degreesToRotateSonicSensor = 100;
	static final boolean debug = true;
	static BTConnection myBTConnection;
	static final int LineThreshold = 400;
	static final int robotSpeed = 100;
	static final int sleepTime = 10000;
	static ServerData myServerData;
	
	public static void sendToServer(RobotData myRobotData)
	{
		if(debug)
			//RobotUtilities.drawString("North " + myRobotData.distanceNorth,"South " + myRobotData.distanceSouth,"East "+myRobotData.distanceEast,"West "+myRobotData.distanceWest);
		myRobotDataParser.writeData(myBTConnection.openDataOutputStream(), myRobotData);
	}
	
	public static ServerData getInstructionsFromServer()
	{
		if(!debug)
			RobotUtilities.drawString("Waiting For Data");
		ServerData myServerData = myServerDataParser.readData(myBTConnection.openDataInputStream());
		return myServerData;
	}
	
	public static void executeInstruction(int newHeading)
	{
		if(newHeading != RobotHeading.WAIT)
		{
			int destinationHeading = newHeading - currentHeading;
			
			//This is a simple optimization
			if(destinationHeading == -270)
				destinationHeading = 90;
			if(destinationHeading == 270)
				destinationHeading = -90;
			
			currentHeading = newHeading;
			MoveToNewDestination(destinationHeading);
		}
		//Else -just sit and wait.
	}
	public static void MoveToNewDestination(int degreesToRotate)
	{
		Boolean IHaveNotTouchedTheLine = true;
		myPilot.setSpeed(robotSpeed);
		
		if(degreesToRotate != 0)
			myPilot.rotate(-degreesToRotate);
		
		while(IHaveNotTouchedTheLine)
		{
			if(debug)
				
				RobotUtilities.drawString("Left: " + leftLightSensor.getNormalizedLightValue(), "Right: " + rightLightSensor.getNormalizedLightValue());
			if(leftLightSensor.getNormalizedLightValue() < LineThreshold && rightLightSensor.getNormalizedLightValue()< LineThreshold)
			{
				Sound.beep();
				IHaveNotTouchedTheLine = false;
				myPilot.stop();
			}
			if(leftLightSensor.getNormalizedLightValue() < LineThreshold)
			{
				myPilot.getRight().stop();
				myPilot.getLeft().forward();
			}
			if(rightLightSensor.getNormalizedLightValue()< LineThreshold)
			{
				myPilot.getLeft().stop();
				myPilot.getRight().forward();
				
			}
			if(leftLightSensor.getNormalizedLightValue() > LineThreshold && rightLightSensor.getNormalizedLightValue() > LineThreshold)
			{
				myPilot.backward();
			}
		}
		myPilot.travel(-halfCellLength);
		Delay.msDelay(sleepTime/3);
		Sound.beep();
	}
	
	public static RobotData senseEnvironment()
	{
		RobotData tempData = null;
		
		int left = getDistance(degreesToRotateSonicSensor);
		int forward = getDistance(degreesToRotateSonicSensor);
		int right = getDistance(-degreesToRotateSonicSensor * 2);
		int backward = 255;

		RobotUtilities.drawString("L"+left, "F:"+forward, "R:"+right);

		switch(currentHeading)
		{
		case RobotHeading.NORTH:
			tempData = new RobotData(forward,right,left,backward,currentHeading,0);
			break;
		case RobotHeading.EAST:
			tempData = new RobotData(left,forward,backward,right,currentHeading,0);
			break;
		case RobotHeading.SOUTH:
			tempData = new RobotData(backward,left,right,forward,currentHeading,0);
			break;
		case RobotHeading.WEST:
			tempData = new RobotData(right,backward,forward,left,currentHeading,0);
			break;
		}
		return tempData;
		
		//RobotData(int northReading, int eastReading, int westReading, int southReading, int currentDirection, int compassReading)
	}
	
	public static int getDistance(int degreesToRotate)
	{
		ultraSonicSesnor.ping();
		Delay.msDelay(20);
		int distance = ultraSonicSesnor.getDistance();
		Sound.beep();
		topMotor.rotate(degreesToRotate);
		return distance;
	}
		
	public static void waitForServerConnection()
	{
		RobotUtilities.drawString("Waiting Server");
		myBTConnection = Bluetooth.waitForConnection();
		RobotUtilities.drawString("Conn Successful!");
	}
	
	public static void BabyJustDance()
	{
		Random random = new Random();
		myPilot.setSpeed(robotSpeed);
		Sound.beep();
		myPilot.rotate(random.nextInt(75));
		Sound.beep();
		myPilot.rotate(-random.nextInt(75));
		Sound.beep();
		myPilot.rotate(random.nextInt(75));
		Sound.beep();
		
	}
	public GridMapperClient()
	{
		//Initiate Bluetooth Comms	
		iAmAlive = true;	
		leftLightSensor = new LightSensor(SensorPort.S4);
		rightLightSensor = new LightSensor(SensorPort.S1);
		ultraSonicSesnor = new UltrasonicSensor(SensorPort.S3);
		myRobotDataParser = new RobotDataParser();
		myServerDataParser = new ServerDataParser();
		myPilot = new TachoPilot(2.65f,5.6f, Motor.A, Motor.B, true);	
		
		waitForServerConnection();
		while(iAmAlive)
		{
			RobotData myRobotData = senseEnvironment();
			
			try{sendToServer(myRobotData);
			myServerData = getInstructionsFromServer();}
			catch(Exception e)			
			{
				System.exit(0);
			}
			
			if(myServerData.alive)
			{
				if(myServerData.robotHeading == RobotHeading.WAIT)
				{
					RobotUtilities.drawString("Sleeping...");
					Delay.msDelay(sleepTime);
				}
				else
					executeInstruction(myServerData.robotHeading);
			}
			else
				iAmAlive = false;
		}
		BabyJustDance();
	}
	
	public static void main(String[] args) throws IOException {
		new GridMapperClient();
	}
}
